package nezumi.util;

/**
 * A library of commonly used functions for working with angles.
 * 
 * @author rui
 * 
 */
public final class AngleGeometry {

	/**
	 * Compute the difference between two angles, in radians.
	 * 
	 * @param alpha -
	 *            initial angle in radians
	 * @param beta -
	 *            final angle in radians
	 * @return
	 */
	public static double angleDifference(double alpha, double beta) {

		double deltatheta = 0;

		if ((alpha < 0 && beta < 0) || (alpha > 0 && beta > 0)) {
			deltatheta = beta - alpha;
		} else {
			if (Math.abs(alpha) + Math.abs(beta) <= Math.PI)
				deltatheta = (Math.abs(alpha) + Math.abs(beta));
			else
				deltatheta = -Math.abs(2 * Math.PI - Math.abs(alpha - beta));
			if (beta < alpha)
				deltatheta *= -1;
		}
		return deltatheta;
	}

	/**
	 * Turn an angle in the [-PI,PI] into a angle in the interval [0,2PI]
	 * 
	 * @param angle
	 *            in radians
	 * @return
	 */
	public static double makePositive(double angle) {
		return angle >= 0 ? angle : angle + 2 * Math.PI;
	}

	/**
	 * Normalizes an angle to the interval [-PI,PI].
	 * @param angle
	 * @return
	 */
	public static double normalizeAngle(double angle) {

		// let us make sure that orientation is in the expected interval
		assert angle <= 2 * Math.PI && angle >= -2 * Math.PI : "orientation = "
				+ Math.toDegrees(angle);

		if (angle < -Math.PI)
			angle = 2 * Math.PI + angle;
		else if (angle > Math.PI)
			angle = -1 * (2 * Math.PI - angle);

		assert angle <= Math.PI && angle >= -Math.PI : "normalized orientation = "
				+ Math.toDegrees(angle);

		return angle;
	}

	
}
